package com.rockwell.sniffyhunter.model;

import com.rockwell.sniffyhunter.modbus.ModbusSerialClient;
import com.rockwell.sniffyhunter.utils.IRobotSensors;

import net.wimpi.modbus.ModbusException;
import net.wimpi.modbus.ModbusIOException;
import net.wimpi.modbus.ModbusSlaveException;
import net.wimpi.modbus.facade.ModbusSerialMaster;
import net.wimpi.modbus.procimg.InputRegister;
import net.wimpi.modbus.util.BitVector;
import net.wimpi.modbus.ModbusException;

public class RobotSensors implements IRobotSensors {
	private static final int UNIT_ID = 1;	// unit id of the modbus slave
	
	private float orientation = 0.0f;

	ModbusSerialClient m_modbusMaster;
	
	public RobotSensors(ModbusSerialClient modbusMaster) {
		m_modbusMaster = modbusMaster;
	}
	
	@Override
	public boolean hasLeftDistanceSensor() {
		return true;	// should be read from settings
	}
	
	@Override
	public boolean hasFrontDistanceSensor() {
		return true;	// should be read from settings
	}
	
	@Override
	public boolean hasRightDistanceSensor() {
		return true;	// should be read from settings
	}
	
	@Override
	public double getLeftDistance() {
		try {
			return (readInt(LEFT_DISTANCE_SENSOR1_REF) + LEFT_SENSOR_DATUM_DIFFERENCE);
		} catch (ModbusException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
			return 0;
		}
	}
	
	@Override
	public int[] getLeftDistances() {
		int[] values = null;
		int i = 0;
		while (values == null) {
		try {
			
			if (i > 1000000)
				break;

				values = m_modbusMaster.readHoldingRegisters(LEFT_DISTANCE_SENSOR1_REF, LEFT_DISTANCE_SENSOR2_REF);
				++i;
				if (values == null)
					continue;
				
				values[0] += LEFT_SENSOR_DATUM_DIFFERENCE;
			
		} catch (ModbusIOException e) {
			// TODO Auto-generated catch block
			++i;
			e.printStackTrace();
		} catch (ModbusSlaveException e) {
			// TODO Auto-generated catch block
			++i;
			e.printStackTrace();
		} catch (ModbusException e) {
			// TODO Auto-generated catch block
			++i;
			e.printStackTrace();
		}
		}
		return values;
	}
	
	@Override
	public boolean detectFrontWall() {
		return getFrontStatus();
	}
	
	@Override
	public boolean getFrontStatus() {
		return readSensorStatus(SENSORS_STATUS_REF, FRONT_SENSOR_MASK);
	}
	
	@Override
	public boolean detectLeftWall() {
		return readSensorStatus(SENSORS_STATUS_REF, LEFT_SENSOR1_MASK);
	}

	@Override
	public boolean detectRightWall() {
		return readSensorStatus(SENSORS_STATUS_REF, RIGHT_SENSOR1_MASK);
	}
	
	@Override
	public boolean isArmUp() {
		return !readSensorStatus(SENSORS_STATUS_REF, ARM_DOWN_SENSOR_MASK);
	}
	
	@Override
	public boolean isArmDown() {
		return readSensorStatus(SENSORS_STATUS_REF, ARM_DOWN_SENSOR_MASK);
	}
	
	@Override
	public boolean isWheelMoving() {
		return readSensorStatus(SENSORS_STATUS_REF, WHEEL_STATE_MASK);
	}
	
	private boolean readSensorStatus(int ref, short mask) {
		try {
			int read = readInt(ref) & mask;
			return (read != 0);
		} catch (ModbusException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
			return false;
		}
	}
	
	@Override
	public double getRightDistance() {
		try {
			return (readInt(RIGHT_DISTANCE_SENSOR1_REF) + RIGHT_SENSOR_DATUM_DIFFERENCE);
		} catch (ModbusException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
			return 0;
		}
	}
	
	@Override
	public int[] getFrontDistances() {
		int[] values = null;
		int i = 0;
		while (values == null) {
		try {
			if (i > 1000000)
				break;
			values = m_modbusMaster.readHoldingRegisters(FRONT_DISTANCE_SENSOR1_REF, FRONT_DISTANCE_SENSOR3_REF);
			++i;
		} catch (ModbusIOException e) {
			// TODO Auto-generated catch block
			++i;
			e.printStackTrace();
		} catch (ModbusSlaveException e) {
			// TODO Auto-generated catch block
			++i;
			e.printStackTrace();
		} catch (ModbusException e) {
			// TODO Auto-generated catch block
			++i;
			e.printStackTrace();
		}
		}
		return values;
	}
	
	@Override
	public int[] getRightDistances() {
		int[] values = null;
		int i = 0;
		while (values == null) {
		try {
				if (i > 1000000)
				break;
				values = m_modbusMaster.readHoldingRegisters(RIGHT_DISTANCE_SENSOR1_REF, RIGHT_DISTANCE_SENSOR2_REF);
				++i;
				
				if (values == null)
					continue;
				
				values[0] += RIGHT_SENSOR_DATUM_DIFFERENCE;
			
			} catch (ModbusIOException e) {
				// TODO Auto-generated catch block
				++i;
				e.printStackTrace();
			} catch (ModbusSlaveException e) {
			// TODO Auto-generated catch block
				++i;
				e.printStackTrace();
			} catch (ModbusException e) {
			// TODO Auto-generated catch block
				++i;
				e.printStackTrace();
			}
	    }
		
		return values;
	}
	
	@Override
	public float getOrientation() {
		return orientation;
	}
	
	@Override
	synchronized public void setOrientation(float angle) {
		if (Math.abs(orientation - angle) >= 0.5)
		{
			orientation = angle;
		}
	}
	
	private int readInt(int ref) throws ModbusException {
		if (!m_modbusMaster.isConnected()) {
			try {
				m_modbusMaster.connect("COM1");
			} catch (Exception e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}
		
		int v = m_modbusMaster.readHoldingRegister(ref);
		return v;
	}

	@Override
	public int[] getAllSensorStatus() {
		int[] values = null;
		int i = 0;
		while (values == null) {
		try {
			if (i > 1000000)
				break;
			values = m_modbusMaster.readHoldingRegisters(START_OF_SENSORS_STATUS_REF, END_OF_SENSORS_STATUS_REF);
				++i;
		} catch (ModbusIOException e) {
			// TODO Auto-generated catch block
			++i;
			e.printStackTrace();
		} catch (ModbusSlaveException e) {
			// TODO Auto-generated catch block
			++i;
			e.printStackTrace();
		} catch (ModbusException e) {
			// TODO Auto-generated catch block
			++i;
			e.printStackTrace();
		}
		}
		return values;
	}

	@Override
	public int getSensorFlags() {
		int value = 0;
		try {
			value = readInt(SENSORS_STATUS_REF);
		} catch (ModbusIOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		} catch (ModbusSlaveException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		} catch (ModbusException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		return value;
	}

	@Override
	public double getActiveEncoderValue() {
		try {
			return readInt(ACTIVE_MOTOR_ENCODER_REF);
		} catch (ModbusException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
			return 0;
		}
	}
}
